#ifndef PID_H
#define PID_H

#include "main.h"
#include "stdint.h"
#include "stdlib.h"
#include "user_lib.h"

/********************************** COMMON PID **********************************/
enum PID_MODE
{
    PID_POSITION = 0,
    PID_DELTA
};

typedef struct
{
    uint8_t mode;
    fp32 Kp;
    fp32 Ki;
    fp32 Kd;

    fp32 max_out;
    fp32 max_iout;

    fp32 set;
    fp32 fdb;

    fp32 out;
    fp32 Pout;
    fp32 Iout;
    fp32 Dout;
    fp32 Dbuf[3];
    fp32 error[3];

} pid_type_def;

/**
  * @brief          初始化 PID 控制器参数
  * @author         冉文治
  * @param          pid 指向 `pid_type_def` 结构体的指针，用于存储 PID 控制器的当前状态
  * @param          mode PID 控制器的工作模式，例如普通模式、反向模式等
  * @param          PID 一个包含三个元素的数组，表示 PID 控制器的增益系数 `[Kp, Ki, Kd]`
  * @param          max_out 最大输出限制，防止输出过大导致控制不稳定
  * @param          max_iout 最大积分输出限制，防止积分饱和
  * @retval         无
  *
  * @note           该函数会将 PID 控制器的各个参数初始化为传入值，并将相关状态变量（如误差、输出）清零。
  */
extern void PID_init(pid_type_def *pid, uint8_t mode, const fp32 PID[3], fp32 max_out, fp32 max_iout);

/**
  * @brief          计算 PID 控制器的输出
  * @author         冉文治
  * @param          pid 指向 `pid_type_def` 结构体的指针，包含 PID 控制器的当前状态和参数
  * @param          ref 当前的反馈值，通常是传感器获取的测量值
  * @param          set 目标设定值，即期望值
  * @retval         返回计算后的 PID 输出值，用于控制量的调节
  *
  * @note           此函数根据当前 PID 控制器的工作模式（位置控制或增量控制）进行不同的计算：
  *                 - 在位置控制模式下，PID 控制器基于设定值和反馈值的差计算 PID 输出。
  *                 - 在增量控制模式下，PID 控制器基于前后误差的变化进行控制。
  *                 此外，PID 控制器的输出和积分输出会受到最大输出限制。
  */
extern fp32 PID_calc(pid_type_def *pid, fp32 ref, fp32 set);

/**
  * @brief          清除 PID 控制器的状态和历史数据
  * @author         冉文治
  * @param          pid 指向 `pid_type_def` 结构体的指针，包含 PID 控制器的当前状态
  * @retval         无返回值
  *
  * @note           此函数用于将 PID 控制器的所有状态变量重置为 0。主要用于重置 PID 控制器的历史误差、微分缓存和计算结果等，以便开始新的控制周期。
  *                 - `error[]` 数组：清除历史误差。
  *                 - `Dbuf[]` 数组：清除微分缓存。
  *                 - `Pout`、`Iout`、`Dout` 和 `out`：清除比例、积分、微分输出和总输出。
  *                 - `fdb` 和 `set`：清除反馈值和设定值。
  */
extern void PID_clear(pid_type_def *pid);

/**
  * @brief          更改 PID 控制器的参数（Kp、Ki、Kd）
  * @author         冉文治
  * @param          pid 指向 `pid_type_def` 结构体的指针，包含 PID 控制器的当前状态
  * @param          Kp 目标比例增益（Proportional Gain）
  * @param          Ki 目标积分增益（Integral Gain）
  * @param          Kd 目标微分增益（Derivative Gain）
  * @retval         无返回值
  *
  * @note           此函数用于动态更改 PID 控制器的增益参数（Kp、Ki、Kd）。在某些情况下，PID 控制器的参数可能需要调整，例如：控制系统的负载变化，或为了更好地响应特定的控制目标。
  *                 修改参数后，新的增益将立即生效，但不会影响控制器的历史状态（例如误差、输出等）。
  */
extern void PID_change(pid_type_def *pid, fp32 Kp, fp32 Ki, fp32 Kd, fp32 max_out, fp32 max_iout);

/******************************** FUZZY PID **********************************/
#ifndef abs
#define abs(x) ((x > 0) ? x : -x)
#endif

#ifndef user_malloc
#ifdef _CMSIS_OS_H
#define user_malloc pvPortMalloc
#else
#define user_malloc malloc
#endif
#endif

#define NB -3
#define NM -2
#define NS -1
#define ZE 0
#define PS 1
#define PM 2
#define PB 3

typedef __packed struct
{
    fp32 KpFuzzy;
    fp32 KiFuzzy;
    fp32 KdFuzzy;

    fp32 (*FuzzyRuleKp)[7];
    fp32 (*FuzzyRuleKi)[7];
    fp32 (*FuzzyRuleKd)[7];

    fp32 KpRatio;
    fp32 KiRatio;
    fp32 KdRatio;

    fp32 eStep;
    fp32 ecStep;

    fp32 e;
    fp32 ec;
    fp32 eLast;

    uint32_t DWT_CNT;
    fp32 dt;
} FuzzyRule_t;

/**
  * @brief          初始化模糊控制规则参数
  * @author         冉文治
  * @param          fuzzyRule 指向 `FuzzyRule_t` 结构体的指针，包含模糊控制器的相关参数和规则
  * @param          fuzzyRuleKp 指向一个 `7x7` 大小的二维数组，表示模糊控制器中与比例增益（Kp）相关的模糊规则（可以传入自定义规则数组，若传入 NULL，则使用默认规则）
  * @param          fuzzyRuleKi 指向一个 `7x7` 大小的二维数组，表示模糊控制器中与积分增益（Ki）相关的模糊规则（可以传入自定义规则数组，若传入 NULL，则使用默认规则）
  * @param          fuzzyRuleKd 指向一个 `7x7` 大小的二维数组，表示模糊控制器中与微分增益（Kd）相关的模糊规则（可以传入自定义规则数组，若传入 NULL，则使用默认规则）
  * @param          kpRatio 比例增益的模糊调整系数（范围为0到1）
  * @param          kiRatio 积分增益的模糊调整系数（范围为0到1）
  * @param          kdRatio 微分增益的模糊调整系数（范围为0到1）
  * @param          eStep 每步误差的增量，决定了误差的分辨率（误差步长）
  * @param          ecStep 每步误差变化量的增量，决定了误差变化的分辨率（误差变化步长）
  * @retval         无返回值
  *
  * @note           此函数用于初始化模糊控制规则及相关参数。它允许用户传入自定义的模糊规则（Kp、Ki、Kd），若未传入，则使用默认的规则阵列。
  *                 同时，用户可以调整模糊控制器的增益比例系数（`kpRatio`、`kiRatio`、`kdRatio`）和误差步长（`eStep` 和 `ecStep`），
  *                 这些参数控制模糊规则的精度和控制响应速度。若 `eStep` 或 `ecStep` 设置过小，则默认值为 1。
  */
extern void Fuzzy_Rule_Init(FuzzyRule_t *fuzzyRule, fp32 (*fuzzyRuleKp)[7], fp32 (*fuzzyRuleKi)[7], fp32 (*fuzzyRuleKd)[7], fp32 kpRatio, fp32 kiRatio, fp32 kdRatio, fp32 eStep, fp32 ecStep);
/**
  * @brief          实现模糊控制器的规则计算，基于当前的误差（e）和误差变化率（ec）计算比例（Kp）、积分（Ki）和微分（Kd）增益。
  * @author         冉文治
  * @param          fuzzyRule  指向 `FuzzyRule_t` 结构体的指针，包含模糊控制器的当前状态和参数
  * @param          measure    当前测量值（反馈量）
  * @param          ref        设定值（目标值）
  * @retval         无返回值。结果保存在 `fuzzyRule` 结构体中，更新了 `KpFuzzy`、`KiFuzzy` 和 `KdFuzzy`。
  *
  * @note           该函数根据当前的误差（e）和误差变化率（ec），通过模糊规则阵列计算相应的模糊控制增益 Kp、Ki 和 Kd。
  *                 模糊推理使用了误差和误差变化率的当前值及其在模糊规则中的索引，利用模糊控制增益的加权平均进行计算。
  *                 最终的模糊控制增益将用于调整 PID 控制器的输出。
  */
extern void Fuzzy_Rule_Implementation(FuzzyRule_t *fuzzyRule, fp32 measure, fp32 ref);

/******************************* PID CONTROL *********************************/
typedef enum pid_Improvement_e
{
    NONE = 0X00,                        //0000 0000
    Integral_Limit = 0x01,              //0000 0001
    Derivative_On_Measurement = 0x02,   //0000 0010
    Trapezoid_Intergral = 0x04,         //0000 0100
    Proportional_On_Measurement = 0x08, //0000 1000
    OutputFilter = 0x10,                //0001 0000
    ChangingIntegrationRate = 0x20,     //0010 0000
    DerivativeFilter = 0x40,            //0100 0000
    ErrorHandle = 0x80,                 //1000 0000
} PID_Improvement_e;

typedef enum errorType_e
{
    PID_ERROR_NONE = 0x00U,
    Motor_Blocked = 0x01U
} ErrorType_e;

typedef __packed struct
{
    uint64_t ERRORCount;
    ErrorType_e ERRORType;
} PID_ErrorHandler_t;

typedef __packed struct pid_t
{
    fp32 Ref;
    fp32 Kp;
    fp32 Ki;
    fp32 Kd;

    fp32 Measure;
    fp32 Last_Measure;
    fp32 Err;
    fp32 Last_Err;
    fp32 Last_ITerm;

    fp32 Pout;
    fp32 Iout;
    fp32 Dout;
    fp32 ITerm;

    fp32 Output;
    fp32 Last_Output;
    fp32 Last_Dout;

    fp32 MaxOut;
    fp32 IntegralLimit;
    fp32 DeadBand;
    fp32 ControlPeriod;
    fp32 CoefA;         //For Changing Integral
    fp32 CoefB;         //ITerm = Err*((A-abs(err)+B)/A)  when B<|err|<A+B
    fp32 Output_LPF_RC; // RC = 1/omegac
    fp32 Derivative_LPF_RC;

    uint16_t OLS_Order;
    Ordinary_Least_Squares_t OLS;

    uint32_t DWT_CNT;
    fp32 dt;

    FuzzyRule_t *FuzzyRule;

    uint8_t Improve;

    PID_ErrorHandler_t ERRORHandler;

    void (*User_Func1_f)(struct pid_t *pid);
    void (*User_Func2_f)(struct pid_t *pid);
} PID_t;

void PID_Init
(
    PID_t *pid,
    fp32 max_out,
    fp32 intergral_limit,
    fp32 deadband,

    fp32 kp,
    fp32 ki,
    fp32 kd,

    fp32 A,
    fp32 B,

    fp32 output_lpf_rc,
    fp32 derivative_lpf_rc,

    uint16_t ols_order,

    uint8_t improve
);

/**
  * @brief          计算 PID 控制器的输出
  * @author         冉文治
  * @param          pid                指向 PID 结构体的指针，包含 PID 控制器的状态和参数
  * @param          measure            当前测量值（反馈值）
  * @param          ref                参考值（目标值）
  * @retval         返回 PID 控制器的输出值
  *
  * @note           此函数根据当前的测量值和参考值，计算 PID 控制器的输出。控制器计算过程包括以下步骤：
  *                 - 计算误差（参考值与测量值之差）
  *                 - 根据误差、增益（Kp、Ki、Kd）、时间间隔等计算 PID 控制器的各项输出（P、I、D）
  *                 - 支持模糊控制和误差处理，包含增益优化、积分限幅、微分滤波等功能
  *                 - 支持积分限幅、积分率调整、微分计算等功能，确保控制器的稳定性和精度
  *                 - 最终输出根据 PID 控制器的参数、改善标志和外部回调函数进行调整
  */
extern fp32 PID_Calculate(PID_t *pid, fp32 measure, fp32 ref);

/*************************** FEEDFORWARD CONTROL *****************************/

typedef __packed struct
{
    fp32 c[3]; // G(s) = 1/(c2s^2 + c1s + c0)

    fp32 Ref;
    fp32 Last_Ref;

    fp32 DeadBand;

    uint32_t DWT_CNT;
    fp32 dt;

    fp32 LPF_RC; // RC = 1/omegac

    fp32 Ref_dot;
    fp32 Ref_ddot;
    fp32 Last_Ref_dot;

    uint16_t Ref_dot_OLS_Order;
    Ordinary_Least_Squares_t Ref_dot_OLS;
    uint16_t Ref_ddot_OLS_Order;
    Ordinary_Least_Squares_t Ref_ddot_OLS;

    fp32 Output;
    fp32 MaxOut;

} Feedforward_t;

void Feedforward_Init
(
    Feedforward_t *ffc,
    fp32 max_out,
    fp32 *c,
    fp32 lpf_rc,
    uint16_t ref_dot_ols_order,
    uint16_t ref_ddot_ols_order
);

/**
  * @brief          计算前馈控制器的输出
  * @author         冉文治
  * @param          ffc               指向前馈控制器结构体的指针，包含当前的控制器状态和参数
  * @param          ref               参考信号
  * @retval         前馈控制器的输出值
  *
  * @note           此函数根据参考信号及其导数（速度、加速度）计算前馈控制器的输出值。
  *                 参考信号的值会经过低通滤波处理，然后计算其一阶和二阶导数（速度和加速度）。
  *                 最后，输出值根据预设的系数进行加权求和，并限制在最大输出范围内。
  */
extern fp32 Feedforward_Calculate(Feedforward_t *ffc, fp32 ref);

/************************* LINEAR DISTURBANCE OBSERVER *************************/
typedef __packed struct
{
    fp32 c[3]; // G(s) = 1/(c2s^2 + c1s + c0)

    fp32 Measure;
    fp32 Last_Measure;

    fp32 u; // system input

    fp32 DeadBand;

    uint32_t DWT_CNT;
    fp32 dt;

    fp32 LPF_RC; // RC = 1/omegac

    fp32 Measure_dot;
    fp32 Measure_ddot;
    fp32 Last_Measure_dot;

    uint16_t Measure_dot_OLS_Order;
    Ordinary_Least_Squares_t Measure_dot_OLS;
    uint16_t Measure_ddot_OLS_Order;
    Ordinary_Least_Squares_t Measure_ddot_OLS;

    fp32 Disturbance;
    fp32 Output;
    fp32 Last_Disturbance;
    fp32 Max_Disturbance;
} LDOB_t;

void LDOB_Init
(
    LDOB_t *ldob,
    fp32 max_d,
    fp32 deadband,
    fp32 *c,
    fp32 lpf_rc,
    uint16_t measure_dot_ols_order,
    uint16_t measure_ddot_ols_order
);

/**
  * @brief          计算 LDOB (Luenberger Disturbance Observer) 控制器的输出
  * @author         冉文治
  * @param          ldob               指向 LDOB 控制器结构体的指针，包含当前的控制器状态和参数
  * @param          measure            测量信号
  * @param          u                  控制输入
  * @retval         控制器的输出值（估计的干扰）
  *
  * @note           此函数根据测量信号及其导数（速度、加速度）估算系统的干扰并输出。
  *                 测量信号会经过低通滤波器进行平滑，并计算其一阶和二阶导数（速度和加速度）。
  *                 然后根据系数计算干扰估计值，输出会受到最大干扰值的限制。
  *                 如果估计的干扰大于死区范围，则输出该干扰值，否则输出 0。
  */
extern fp32 LDOB_Calculate(LDOB_t *ldob, fp32 measure, fp32 u);

/*************************** Tracking Differentiator ***************************/
typedef __packed struct
{
    fp32 Input;

    fp32 h0;
    fp32 r;

    fp32 x;
    fp32 dx;
    fp32 ddx;

    fp32 last_dx;
    fp32 last_ddx;

    uint32_t DWT_CNT;
    fp32 dt;
} TD_t;

/**
  * @brief          初始化TD控制器（Thermal Diffusion Controller）的参数
  * @author         冉文治
  * @param          td      指向 TD 控制器结构体的指针，用于存储初始化后的控制器状态
  * @param          r       控制器的系数 r，用于调节热扩散的控制力度
  * @param          h0      初始的热扩散值 h0，用于控制器的初始化
  * @retval         无返回值。初始化后的 TD 控制器将保存在 `td` 结构体中。
  *
  * @note           此函数用于初始化 TD 控制器的各项参数，包括控制系数 `r` 和初始值 `h0`，
  *                 并重置状态变量（`x`, `dx`, `ddx`等）。初始化时，`x`, `dx`, `ddx`等状态
  *                 变量被设为零，以便控制器在后续计算中使用。
  */
extern void TD_Init(TD_t *td, fp32 r, fp32 h0);

/**
  * @brief          计算并更新 TD 控制器的输出
  * @author         冉文治
  * @param          td      指向 TD 控制器结构体的指针，包含当前的控制器状态和参数
  * @param          input   输入信号，用于控制器计算
  * @retval         控制器的输出值
  *
  * @note           此函数根据输入信号 `input` 和控制器的参数计算新的状态值，并返回控制器的输出。
  *                 控制器使用热扩散算法进行计算。首先，根据输入信号更新 `x`, `dx`, `ddx` 等状态变量，
  *                 然后返回新的状态值 `x` 作为控制输出。
  */
extern fp32 TD_Calculate(TD_t *td, fp32 input);

#endif
